#include <base\dprotation.h>
#include <base\dpmatrix.h>

#include <cmath>

dprotation::dprotation(const double q[4]) {

  this->quat[0] = q[0];
  this->quat[1] = q[1];
  this->quat[2] = q[2];
  this->quat[3] = q[3];
  this->quat.normalize();
}

dprotation::dprotation(const vec3d & axis, const double radians) {
  quat[3] = static_cast<double>(cos(radians/2));

  const double sineval = static_cast<double>(sin(radians/2));
  vec3d a = axis;

  // we test for a null vector above
  (void) a.normalize();
  quat[0] = a[0] * sineval;
  quat[1] = a[1] * sineval;
  quat[2] = a[2] * sineval;
}

dprotation::dprotation(const double q0, const double q1, const double q2, const double q3) {
  quat.x = q0;
  quat.y = q1;
  quat.z = q2;
  quat.w = q3;
}

dprotation 
dprotation::identity(void) {
  return dprotation(0.0, 0.0, 0.0, 1.0);
}

void
dprotation::getValue(dpmatrix & matrix_) const {
  double l = this->quat.length();
  double x,y,z,w;
  if (l > FLT_EPSILON) {
    // normalize it
    x = quat.x / l;
    y = quat.y / l;
    z = quat.z / l;
    w = quat.w / l;
  }
  else {
   // identity
    x = y = z = 0.0;
    w = 1.0;
  }

  matrix_[0][0] = 1.0 - 2.0 * (y * y + z * z);
  matrix_[0][1] = 2.0 * (x * y + z * w);
  matrix_[0][2] = 2.0 * (z * x - y * w);
  matrix_[0][3] = 0.0;

  matrix_[1][0] = 2.0 * (x * y - z * w);
  matrix_[1][1] = 1.0 - 2.0 * (z * z + x * x);
  matrix_[1][2] = 2.0 * (y * z + x * w);
  matrix_[1][3] = 0;

  matrix_[2][0] = 2.0 * (z * x + y * w);
  matrix_[2][1] = 2.0 * (y * z - x * w);
  matrix_[2][2] = 1.0 - 2.0 * (y * y + x * x);
  matrix_[2][3] = 0.0;

  matrix_[3][0] = 0.0;
  matrix_[3][1] = 0.0;
  matrix_[3][2] = 0.0;
  matrix_[3][3] = 1.0;
}

const double *
dprotation::getValue(void) const {
  return &this->quat.x;
}